﻿#include "StdAfx.h"
#include <math.h>
#include "common.h"



Pose genPose(double x, double y, double z, double rx, double ry, double rz)
{
    Pose pp;
    pp.x = x;
    pp.y = y;
    pp.z = z;
    pp.rx = rx;
    pp.ry = ry;
    pp.rz = rz;
    return pp;
}

Move genMove(double x, double y, double z, double rx,double ry, double rz,
             int moveType, int speed)
{
    Move m;
    m.p.x = x;
    m.p.x = x;
    m.p.y = y;
    m.p.z = z;
    m.p.rx = rx;
    m.p.ry = ry;
    m.p.rz = rz;
    m.moveType = moveType;
    m.speed = speed;
    return m;
}

Move genMove(Pose p, int moveType, int speed)
{
    Move m;
    m.p = p;
    m.moveType = moveType;
    m.speed = speed;
    return m;
}

cv::Mat MatPose(Pose p)
{
    double pi = 3.1415926;
    //输入进来是角度，现转换成弧度
    p.rx = p.rx * pi / 180;
    p.ry = p.ry * pi / 180;
    p.rz = p.rz * pi /180;
    cv::Mat m(4,4,CV_64F);
    cv::Mat matRx,matRy,matRz;
    cv::Mat matR(3, 3, CV_64F);

    matRx = MatRx(p.rx);  // 3*3
    matRy = MatRy(p.ry);
    matRz = MatRz(p.rz);
    cv::gemm(matRz,matRy,1,NULL,0,matR);
    cv::gemm(matR,matRx,1,NULL,0,matR);

 //   cv::FileStorage fs("matR.xml", cv::FileStorage::WRITE);
 //   fs<<"matRx"<<matRx<<"matRy"<<matRy<<"matRz"<<matRz<<"matR"<<matR;
 //   fs.release();

    int i,j;
    for(i=0;i<3;i++)
	{
		for (j = 0; j < 3; j++)
		{
			m.at<double>(i, j) = matR.at<double>(i, j);
		}
	}
    m.at<double>(0,3) = p.x;
    m.at<double>(1,3) = p.y;
    m.at<double>(2,3) = p.z;
    m.at<double>(3,3) = 1;
    m.at<double>(3,0) = m.at<double>(3,1) = m.at<double>(3,2) = 0;
    return m;
}

cv::Mat MatRx(double rx)
{
    cv::Mat m(3,3,CV_64F);
    m.at<double>(0,0) = 1;
    m.at<double>(0,1) = m.at<double>(0,2) = m.at<double>(1,0)
            = m.at<double>(2,0) = 0;
    m.at<double>(1,1) = cos(rx);
    m.at<double>(1,2) = -sin(rx);
    m.at<double>(2,1) = sin(rx);
    m.at<double>(2,2) = cos(rx);
    return m;
}

cv::Mat MatRy(double ry)
{
    cv::Mat m(3,3,CV_64F);
    m.at<double>(1,1) = 1;
    m.at<double>(0,1) = m.at<double>(1,0) = m.at<double>(2,1)
            = m.at<double>(1,2) = 0;
    m.at<double>(0,0) = cos(ry);
    m.at<double>(2,0) = -sin(ry);
    m.at<double>(0,2) = sin(ry);
    m.at<double>(2,2) = cos(ry);
    return m;
}

cv::Mat MatRz(double rz)
{
    cv::Mat m(3,3,CV_64F);
    m.at<double>(2,2) = 1;
    m.at<double>(0,2) = m.at<double>(1,2) = m.at<double>(2,0)
            = m.at<double>(2,1) = 0;
    m.at<double>(0,0) = cos(rz);
    m.at<double>(0,1) = -sin(rz);
    m.at<double>(1,0) = sin(rz);
    m.at<double>(1,1) = cos(rz);
    return m;
}

Pose PoseMat(cv::Mat m)
{
    Pose p;
    p.x = m.at<double>(0,3);
    p.y = m.at<double>(1,3);
    p.z = m.at<double>(2,3);

    double oz = m.at<double>(2,1);
    double az = m.at<double>(2,2);
    double ny = m.at<double>(1,0);
    double nx = m.at<double>(0,0);
    double nz = m.at<double>(2,0);

    double rx1 = atan2(oz,az);   //绕x轴的旋转角（弧度）
    double rz1 = atan2(ny,nx);   //绕z轴的旋转角

    double rx2 = atan2(-oz,-az);
    double rz2 = atan2(-ny,-nx);

    double ry1 = atan2(-nz,nx*cos(rz1)+ny*sin(rz1));   //绕y轴的旋转角
    double ry2 = atan2(-nz,nx*cos(rz2)+ny*sin(rz2));

    if(checkSolution(m, rx1, ry1, rz1))
    {
        p.rx = rx1 * 57.29578;        //弧度转化为角度
        p.ry = ry1 * 57.29578;
        p.rz = rz1 * 57.29578;
    }
    else if(checkSolution(m, rx2, ry2, rz2))
    {
        p.rx = rx1 * 57.29578;
        p.ry = ry1 * 57.29578;
        p.rz = rz1 * 57.29578;
    }

    return p;
}

bool checkSolution(cv::Mat m, double rx, double ry, double rz)
{
    double ox = m.at<double>(0,1);
    double ax = m.at<double>(0,2);
    double oy = m.at<double>(1,1);
    double ay = m.at<double>(1,2);

    double ox2 = cos(rz)*sin(ry)*sin(rx) - sin(rz)*cos(rx);
    double ax2 = cos(rz)*sin(ry)*cos(rx) + sin(rz)*sin(rx);
    double oy2 = sin(rz)*sin(ry)*sin(rx) + cos(rz)*cos(rx);
    double ay2 = sin(rz)*sin(ry)*cos(rx) - cos(rz)*sin(rx);

    double th = 0.1;
    if(abs(ox-ox2)<th && abs(ax-ax2)<th && abs(oy-oy2)<th && abs(ay-ay2)<th)
        return true;
    else
        return false;
}

//两个位姿的相似度
//由于位置和角度的量纲是不一样的，所以必须设定不同的权重
double distPoseT(Pose p1, Pose p2)
{
    double dstP = (p1.x-p2.x) * (p1.x-p2.x)
            + (p1.y-p2.y) * (p1.y-p2.y)
            + (p1.z-p2.z) * (p1.z-p2.z);

    double dst = sqrt(dstP);
    return dst;
}

double distPoseR(Pose p1, Pose p2)
{
    double dstR = (p1.rx-p2.rx) * (p1.rx-p2.rx)
            + (p1.ry-p2.ry) * (p1.ry-p2.ry)
            + (p1.rz-p2.rz) * (p1.rz-p2.rz);

    double dst = sqrt(dstR);
    return dst;
}

std::string double2Str(double a)
{
    std::stringstream ss;
    ss<<a;
    std::string str;
    ss>>str;
    return str;
}

//将一个Pose转换成string
std::string pose2Str(Pose p)
{
    std::string str;
    str += "(" + double2Str(p.x) + ", " + double2Str(p.y) + ", "
            + double2Str(p.z) + ", " + double2Str(p.rx) + ", "
            + double2Str(p.ry) + ", " + double2Str(p.rz) + ")";
    return str;
}


